#include <ros/ros.h>
#include <tf2/utils.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include "perception/costmap_2d/costmap_2d_ros.h"
#include "controller/controller_base.h"
#include "controller/dwa/dwa.h"


class TestDWA{
    public:
        TestDWA(tf2_ros::Buffer* tf_buffer);
        // 循环控制
        void Loop();

    private:
        // 地图回调函数
        void MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg);
        // 二维雷达回调函数
        void LidarCallback(const sensor_msgs::LaserScan::Ptr& msg);
        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStamped::Ptr& msg);
        // 融合数据回调函数
        void FusionDataCallback(const sensor_msgs::PointCloud2::Ptr& msg);

    private:
        ros::NodeHandle nh_;
        tf2_ros::Buffer* const tfBuffer_;
        ros::Subscriber mapSub_; 
        ros::Subscriber lidarScanSub_;
        ros::Subscriber rvizPoseSub_;
        ros::Subscriber fusionDataSub_;
        ros::Publisher LookaheadPosePub_;
        ros::Publisher costmapLocalPub_;
        ros::Publisher costmapGlobalPub_;
        
        boost::shared_ptr<costmap_2d::Costmap2DROS> costmapLocalPtr_;
        boost::shared_ptr<costmap_2d::Costmap2DROS> costmapGlobalPtr_;
        boost::shared_ptr<ControllerBase> localControllerPtr_; 
        boost::shared_ptr<sensor_msgs::PointCloud2> fusionDataPtr_;
        boost::shared_ptr<geometry_msgs::PoseStamped> rvizGoalPtr_;
        
};

TestDWA::TestDWA(tf2_ros::Buffer* tf_buffer):tfBuffer_(tf_buffer),
    fusionDataPtr_(nullptr), rvizGoalPtr_(nullptr){
    costmapGlobalPtr_ = boost::make_shared<costmap_2d::Costmap2DROS>("global_costmap", *tfBuffer_);
    costmapLocalPtr_ = boost::make_shared<costmap_2d::Costmap2DROS>("local_costmap", *tfBuffer_);
    localControllerPtr_ = boost::make_shared<DWAController>();

    costmapGlobalPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/global_costmap", 1);
    costmapLocalPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/local_costmap", 1);
    mapSub_ = nh_.subscribe("/map", 10, &TestDWA::MapCallback, this);
    lidarScanSub_ = nh_.subscribe("/scan_raw", 10, &TestDWA::LidarCallback, this);
    rvizPoseSub_ = nh_.subscribe("/move_base_simple/goal", 10, &TestDWA::RvizPosesCallback, this);
    fusionDataSub_ = nh_.subscribe("/fusion_cloud", 10, &TestDWA::FusionDataCallback, this);
    LookaheadPosePub_ = nh_.advertise<geometry_msgs::PoseStamped>("/lookahead_pose", 10);

    // 循环控制
    Loop();
}

// 地图回调函数
void TestDWA::MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    LOG(INFO) << "[MapCallback] Receive a map. width=" << msg->info.width 
        << ", height=" << msg->info.height
        << ", resolution=" << msg->info.resolution;
    
    // costmapGlobalPtr_->setMapData(msg);
    // costmapLocalPtr_->setMapData(msg);
}

// 二维雷达回调函数
void TestDWA::LidarCallback(const sensor_msgs::LaserScan::Ptr& msg){
    // costmapGlobalPtr_->setMapData(msg);
    // costmapLocalPtr_->setLidar2DScanData(msg);
}

// Rviz位姿回调函数
void TestDWA::RvizPosesCallback(const geometry_msgs::PoseStamped::Ptr& msg){
    LOG(INFO) << "[RvizPosesCallback] Receive a pose. x=" << msg->pose.position.x
        << ", y=" << msg->pose.position.y
        << ", yaw=" << tf2::getYaw(msg->pose.orientation);
    rvizGoalPtr_ = msg;
}

// 融合数据回调函数
void TestDWA::FusionDataCallback(const sensor_msgs::PointCloud2::Ptr& msg){
    fusionDataPtr_ = msg;
    // localControllerPtr_->setPointCloudData(fusionDataPtr_);
}

// 循环控制
void TestDWA::Loop(){
    ros::Rate controller_rate(20);  //20Hz
    nav_msgs::OccupancyGrid local_map;
    nav_msgs::OccupancyGrid global_map;
    while(!ros::isShuttingDown()){
        controller_rate.sleep();
        ros::spinOnce();
        LOG(INFO) << "--------------------------------";

        ros::Time now = ros::Time::now();
        // if(costmapGlobalPtr_){
        //     if((now - global_map.header.stamp).toSec() > 10.0){
        //         costmapGlobalPtr_->updateCostmap();
        //         costmapGlobalPtr_->getOccupancyGrid(global_map);
        //         costmapGlobalPub_.publish(global_map);
        //     }
        // }

        // if(costmapLocalPtr_){
        //     if((now - local_map.header.stamp).toSec() > 0.1){
        //         costmapLocalPtr_->updateCostmap();
        //         costmapLocalPtr_->getOccupancyGrid(local_map);
        //         costmapLocalPub_.publish(local_map);
        //     }
        // }

        if(!rvizGoalPtr_) continue;

        // 获取机器人当前位姿
        geometry_msgs::PoseStamped robotPoseStamp;
        if(!localControllerPtr_->getRobotPose(tfBuffer_, robotPoseStamp)){
            LOG(WARNING) << "can not get robot pose!";
            continue;
        }

        // geometry_msgs::PoseStamped lookahead_pose = 
        //     localControllerPtr_->convertGlobalPoseToLocal(*rvizGoalPtr_, robotPoseStamp);
        // localControllerPtr_->publishLookaheadGoal(lookahead_pose);
        // cout << "lookahead_pose: " << msg_convert::toString(lookahead_pose.pose) << endl;

        // geometry_msgs::Twist cmd_vel;
        // geometry_msgs::Twist curr_vel;
        // mc::status status = 
        //     localControllerPtr_->computeTwist(lookahead_pose, curr_vel, cmd_vel);
        // ROS_INFO_STREAM("--> status: " << status.success << ", "
        //     << ", cmd_vel: " << msg_convert::toString(cmd_vel));
    }
}


int main(int argc, char** argv){
    FLAGS_alsologtostderr = true;           // 日志同时输出到stderr
    FLAGS_max_log_size = 1;                 // 单个日志文件大小上限（MB）, 如果设置为0将默认为1
	FLAGS_stop_logging_if_full_disk = true; // 当磁盘写满时，停止输出
    FLAGS_minloglevel = google::INFO;    // 只记录大于{}等级的日志
    // FLAGS_log_dir = "/home/xxty/user_config/logs";
    google::InitGoogleLogging(argv[0]);

    ros::init(argc, argv, "test_controller");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    ROS_INFO("test_controller start...");

    tf2_ros::Buffer tfBuffer(::ros::Duration(10));
    tf2_ros::TransformListener tfListener(tfBuffer);
    TestDWA test_dwa(&tfBuffer);
    ros::spin();

    google::ShutdownGoogleLogging();

    return 0;
}
